# -*- coding: utf-8 -*-
#解决方案模块，设置了多种类型的解决方案
##################################
import visual.visual as visual
import restoration.arm as arm
import restoration.claw as claw
import restoration.restoration as restoration
import kociemba.resolve as resolve
import restoration.cube as cube
import time
import cv2 as cv
import restoration.command as command

#做测试时，可以把这个置为True
#18 30
test=False

#初始化环境
#包括手臂归零爪子开合状态统一。
def initEnvirment():
    #初始化一个度数
    #command.turnArm(1,r,2);
    #初始化魔方结构体
    command.initCubeNum();
    claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
    arm.toZeroLoc();

def place():
    claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);

#张开爪子
def openClaw():
    claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_OPEN);

#关闭爪子
def closeClaw():
    claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_CLOSE);


#旋转到下一个拍照点
def nextPos(num=1):
    if test:
        return;
    if num==1:#1个摄像头时转动到下一个拍照点
        claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
        arm.ARM_1_Turn(arm.ARM_REVERSAL,90);
        claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
        arm.ARM_1_Turn(arm.ARM_REVERSAL,90);
        arm.ARM_2_Turn(arm.ARM_FORWARD,90);
        claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
        arm.ARM_2_Turn(arm.ARM_FORWARD,90);
    elif num==2:#4个摄像头第一次转动时
        claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
        arm.ARM_1_Turn(arm.ARM_REVERSAL,90);
        claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
        arm.ARM_2_Turn(arm.ARM_FORWARD,60);
    elif num==3:#4个摄像头恢复转动时
        arm.ARM_2_Turn(arm.ARM_REVERSAL,60);
        claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
        arm.ARM_1_Turn(arm.ARM_FORWARD,90);
         
#开始解算并开始还原
def startSolveAndReduction(types):
    if test:
        print("开始解算:");
    start=time.time();
    res=resolve.resolve(types);
    if (res.find("Error")+1):
        raise Exception(res)
    else:
        pass
        if test:
            print("解法："+res);
    end=time.time();
    if test:
        print("解算耗时:",end-start,"秒");
    #开始还原
    if test==False:
        restoration.restorateByStm32(res);
        #打开爪子，放下魔方
        claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_OPEN);
    return res;
#单个摄像头解决方案
def oneCameraSolution(cap):
    #重置下位机的魔方结构
    if test==False:
        command.initCubeNum();
    startTime=time.time();
    #获取第一张图片
    if test==False:
        ret,img1=cap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/test/1.jpg",img1);
    else:
        img1=cv.imread("C:/Users/ASUS/Desktop/test/1.jpg");
    #到下一个位置
    nextPos();
     #获取第二张图片
    if test==False:
        ret,img2=cap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/test/2.jpg",img2);
    else:
        img2=cv.imread("C:/Users/ASUS/Desktop/test/2.jpg");
    #到下一个位置
    nextPos();
     #获取第三张图片
    if test==False:
        ret,img3=cap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/test/3.jpg",img3);
    else:
        img3=cv.imread("C:/Users/ASUS/Desktop/test/3.jpg");
    #得到识别结果
    start=time.time();
    types=visual.getTypesByOneCameraWhite(img1,img2,img3);
    end=time.time();
    print("visual time:",end-start);
    #开始解算并还原
    res=startSolveAndReduction(types);
    #返回结果和用时
    return True,res;

#单个摄像头解决方案(黑色背景魔方)
def oneCameraSolutionWithBlack(cap):
    #重置下位机的魔方结构
    if test==False:
        command.initCubeNum();
    startTime=time.time();
    #获取第一张图片
    if test==False:
        ret,img1=cap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/img/1.jpg",img1);
    else:
        img1=cv.imread("C:/Users/ASUS/Desktop/img/1.jpg");
    #到下一个位置
    nextPos();
     #获取第二张图片
    if test==False:
        ret,img2=cap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/img/2.jpg",img2);
    else:
        img2=cv.imread("C:/Users/ASUS/Desktop/img/2.jpg");
    #到下一个位置
    nextPos();
     #获取第三张图片
    if test==False:
        ret,img3=cap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/img/3.jpg",img3);
    else:
        img3=cv.imread("C:/Users/ASUS/Desktop/img/3.jpg");
    #得到识别结果
    start=time.time();
    if test:
        print("开始识别:");
    types=visual.getTypesByOneCameraBlack(img1,img2,img3);
    end=time.time();
    if test:
        print("识别耗时:",end-start,"秒");
    #开始解算并还原
    res=startSolveAndReduction(types);
    endTime=time.time();
    print("总耗时：",endTime-startTime,"秒")
    #返回结果和用时
    return True,res;

#四个摄像头解决方案(黑色背景魔方)
def fourCameraSolutionWithBlack(topCap,bottomCap,leftCap,rightCap):
    #重置下位机的魔方结构
    if test==False:
        command.initCubeNum();
    startTime=time.time();
    #获取第一张图片
    if test==False:
        ret,top=topCap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/img/top.jpg",top);
    else:
        top=cv.imread("C:/Users/ASUS/Desktop/img/top.jpg");
     #获取第二张图片
    if test==False:
        ret,bottom=bottomCap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/img/bottom.jpg",bottom);
    else:
        bottom=cv.imread("C:/Users/ASUS/Desktop/img/bottom.jpg");
    #到下一个位置
    nextPos(2);
    #获取第三张图片
    if test==False:
        ret,left=leftCap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/img/left.jpg",left);
    else:
        left=cv.imread("C:/Users/ASUS/Desktop/img/left.jpg");
    #获取第四张图片
    if test==False:
        ret,right=rightCap.read();
        cv.imwrite("C:/Users/ASUS/Desktop/img/right.jpg",right);
    else:
        right=cv.imread("C:/Users/ASUS/Desktop/img/right.jpg");
    #恢复之前的位置
    nextPos(3);
    #得到识别结果
    start=time.time();
    print("开始识别:");
    types=visual.getTypesByFourCameraBlack(top,bottom,left,right);
    end=time.time();
    print("识别耗时:",end-start,"秒");
    #开始解算并还原
    res=startSolveAndReduction(types);
    endTime=time.time();
    print("总耗时：",endTime-startTime,"秒");
    #返回结果和用时
    return True,res;

if test:
    oneCameraSolutionWithBlack(1);
'''
if test:
    cap=cv.VideoCapture(1);
    cv.namedWindow("frmae");
    cap.set(cv.CAP_PROP_EXPOSURE, -10);
    while True:
        a=1;
        print("准备抓拍");
        print("当前编号："+str(a));
        while cap.isOpened():
            ret,frame=cap.read();
            cv.imshow("frmae",frame);
            word=cv.waitKey(10);
            if word==ord('q'):
                break;
            elif word==ord('s'):
                print("save");
                cv.imwrite("C:\\Users\\ASUS\\Desktop\\img\\"+str(a)+".jpg",frame);
            elif word==ord('n'):
                a+=1;
                print("当前编号："+str(a));
        print("start");
        oneCameraSolutionWithBlack(0);
'''    
    


    